#include <iostream>
#include "opencv2/myopencv.hpp"
#include "DataReader.h"
#include "DepthTracker.h"
#include <windows.h>
#include "NuiApi.h"
#pragma comment(lib, "Kinect10.lib")


using namespace std;
#define cvSqr(x) (x)*(x)

int main3(void) {
	ColorReader colorReader("color_data.dat", "color_meta.txt");
	CvMat *colorMat = cvCreateMat(colorReader.height,colorReader.width,CV_8UC3);

	DepthReader depthReader("depth_data.dat", "depth_meta.txt");
	CvMat *depthMat = cvCreateMat(depthReader.height,depthReader.width,CV_16SC1);

	for (int i=0; i<0; ++i) {
		colorReader.getNextRec();
		depthReader.getNextRec();
	}

	DepthTracker depthTracker;
	CvRect rect;
	
	INuiSensor *m_pNuiSensor; 
	HRESULT hr = NuiCreateSensorByIndex(0, &m_pNuiSensor);

	WCHAR *id = m_pNuiSensor->NuiDeviceConnectionId();
	hr = m_pNuiSensor->NuiInitialize(NUI_INITIALIZE_FLAG_USES_DEPTH | NUI_INITIALIZE_FLAG_USES_COLOR);


	while (true) {
		depthReader.getNextRec();
		if (depthReader.curRecId == -1) break;
		depthReader.getNextFrame(depthMat->data.s);
		if (!depthTracker.isTracking) {
			rect.x = 225;
			rect.y = 35;
			rect.width = 344 - rect.x;
			rect.height = 171 -rect.y;
			depthTracker.initTracking(depthMat, rect);
		}
		else
			rect = depthTracker.tracking(depthMat);

		cvShowImage("depth", depthMat);

		colorReader.getNextFrame(colorMat->data.ptr);
		cvRectangleR(depthMat, rect, cvScalar(32767));
		cvShowImage("tracker", depthMat);
		cvCircle(colorMat, cvPoint(depthTracker.center.x, depthTracker.center.y),
			cvSqrt(cvSqr(depthTracker.center.x-rect.x)+cvSqr(depthTracker.center.y-rect.y))-20,
			cvScalar(0,0,255));
		cvShowImage("color", colorMat);

		long x,y;
		HRESULT hr = NuiImageGetColorPixelCoordinatesFromDepthPixelAtResolution(
			NUI_IMAGE_RESOLUTION_640x480,
			NUI_IMAGE_RESOLUTION_640x480,
			NULL,
			depthTracker.center.x,depthTracker.center.y,depthTracker.center.z,
			&x,&y
			);
		cout << depthTracker.center.x << " " << depthTracker.center.y << " "
			<< depthTracker.center.z << endl << x << " " << y << endl;

		x = min(480,max(0,x));
		y = min(640,max(0,y));

		cvCircle(colorMat, cvPoint(x, y),
			cvSqrt(cvSqr(depthTracker.center.x-rect.x)+cvSqr(depthTracker.center.y-rect.y))-20,
			cvScalar(0,255,0));
		cvShowImage("color", colorMat);

		cvWaitKey(0);
	}
	return 0;
}
